#include <iostream>

#include "jps.hpp"
#include "tools.hpp"

using namespace std;

Location jump(const Grid& grid, const Location initial, const Location dir, const Location goal)
{
	// new_loc为递进到的新节点
	auto new_loc {initial + dir};
	// initial + dir是否可以通行？
	if(!grid.valid_move(initial, dir)){
		return NoneLoc;
	}
	// 终点也是跳点，返回跳点
	if(new_loc == goal){
		return new_loc;
	}
	// 移动方向为initial -> new_loc，判断new_loc是否有强制邻居
	for(const auto next : grid.pruned_neighbours(new_loc, initial)){
		if(grid.forced(next, new_loc, dir)){
			return new_loc;
		}
	}
	// 当前探索方向为斜向
	if(dir.x != 0 && dir.y !=0){
		// 斜向拆分成2个直线方向
		for(const auto& new_dir: {Location{dir.x, 0}, Location{0, dir.y}}){
			// 沿拆分出来的直线方向前进
			auto jump_point {jump(grid, new_loc, new_dir, goal)};
			if(jump_point != NoneLoc){
				return new_loc;
			}
		}
	}
	// 沿当前方向继续前进
	return jump(grid, new_loc, dir, goal);
}

vector<Location> successors(const Grid& grid, const Location& current, const Location& parent, const Location& goal)
{
	vector<Location> successors;
	auto neighbours {grid.pruned_neighbours(current, parent)};
	// 遍历自然邻居、强制邻居
	for(const auto& n: neighbours){
		// 沿current -> n的方向执行jump()函数
		auto jump_point {jump(grid, current, (n - current).direction(), goal)};
		// 找到跳点了就加入successors
		if(jump_point != NoneLoc){
			successors.push_back(jump_point);
		}
	}
	return successors;
}

unordered_map<Location, Location> jps(const Grid& grid, const Location& start, const Location& goal, heuristic_fn heuristic)
{
	PQLoc open_set;
	unordered_map<Location, Location> came_from {}; // 我们用1个unordered_map去缓存节点之间的parent关系，key为subLoc，value为parent
	unordered_map<Location, double> cost_so_far {}; // cost_so_far缓存的是loc obj的g值

	// 起点加入小根堆，f值 = 0
	open_set.emplace(0, start);
	// 起点g值 = 0
	cost_so_far[start] = 0;
	// 起点无parent
	came_from[start] = NoneLoc;

	// open_set不为空就继续
	while(!open_set.empty()){
		// 小根堆open_set弹出f值最小的loc obj；若找到了终点，提前结束寻路过程
		const auto current {open_set.top().second};
		if(current == goal){
			break;
		}
		open_set.pop();

		Location parent = came_from[current];

		// 方向 = parent -> current，寻找基点current的后继者们
		for(const auto& next : successors(grid, current, parent, goal)){
			// f = g + h
			const auto new_cost = cost_so_far[current] + heuristic(current, next);
			// 先把existing_cost初始化为double类型的最大值
			auto existing_cost = std::numeric_limits<double>::max();
			// 如果先前计算过跳点next的g值了，则赋予existing_cost
			if (cost_so_far.count(next)) {
				existing_cost = cost_so_far.at(next);
			}
			// 尝试更新跳点next的g、h、f值、父节点
			if(cost_so_far.find(next) == cost_so_far.end() || new_cost < existing_cost){
				cost_so_far[next] = new_cost;
				came_from[next] = current;
				// 在优先队列中直接构造并插入1个元素
				open_set.emplace(new_cost + heuristic(next, goal), next);
			}
		}
	}
	return came_from;
}


int main()
{
	unordered_set<Location> walls {
		{1, 1},
		{2, 2}, {2, 3}, {2, 4}, {2, 5}, {2, 6}, {2, 7},
		{4, 5},
		{5, 0}, {5, 1}, {5, 2}, {5, 3}, {5, 4}, {5, 5},
		{6, 5},
		{7, 5},
	};
	Grid map {10, 10, walls};

	Location start {1, 2};
	Location goal {6, 2};

	// 开始寻路，h(n)用的是欧里几得距离
	auto came_from = jps(map, start, goal, Tool::euclidean);

	// 处理并得出找到的通路
	auto path = Tool::reconstruct_path(start, goal, came_from);
	// 打印通路
	Tool::draw_grid(map, {}, {}, path, came_from, start, goal);
}
